#include <iostream>

#include "Types.h"
#include "CubeGenerator.h"
#include "FEMModel.h"
#include "Vector.h"
#include "Triangle.h"
#include "Tetrahedron.h"
#include "Setting.h"

int CubeGenerator::Index3To1 (int i, int j, int k)
{
	return counts_[1] *  counts_[2] * i + counts_[2] * j + k;
}

int CubeGenerator::Index3To1 (IntVector3 id)
{
	return Index3To1 (id[0], id[1], id [2]);
}

bool CubeGenerator::ValidPoint (IntVector3 p)
{
	for (int axis =0; axis < 3; axis++)
		if (p[axis] < 0 || p[axis] >= counts_[axis])
			return false;
	return true;    
}

CubeGenerator::CubeGenerator ()
{
	IntVector3 c(2, 2, 2);
	Real d[3] = {1.0, 1.0, 1.0};
	counts_ = c;
	for (int i = 3;i--;)
		dims_[i] = d[i];
	Init ();
}

void CubeGenerator::Init ()
{
	fem_ = 0;
	nods_.Clear ();
}

int  CubeGenerator::TryGenerateTetra (IntVector3 p0, IntVector3 p1, IntVector3 p2, IntVector3 p3)
{
	IntVector3 p[4];
	p[0] = p0;
	p[1] = p1;
	p[2] = p2;
	p[3] = p3;

	for (int i=0; i < 4; i++)
		if (!ValidPoint (p[i]))
			return 0;


	Node *nodes[4];
	for (int i=0; i < 4; i++)
		nodes[i] = nods_[Index3To1 (p[i])];

	MakePositiveTetrahedron (fem_, nodes);

	return 1;
}

void CubeGenerator::GenerateCubeletVolume (IntVector3 idx)
{
	int x = idx [0];
	int y = idx [1];
	int z = idx [2];


	/*
	So sue me.  I'm too dense to figure out how to do this procedurally.
	*/

	IntVector3 a (x-1,y,  z-1);
	IntVector3 b (x,  y,  z-1);
	IntVector3 c (x,  y,  z);
	IntVector3 d (x-1,y,  z);
	IntVector3 e (x-1,y-1,z-1);
	IntVector3 f (x,  y-1,z-1);
	IntVector3 g (x,  y-1,z);
	IntVector3 h (x-1,y-1,z);

	int tetras_generated=0;

	tetras_generated += TryGenerateTetra (a, f, h, e);
	tetras_generated += TryGenerateTetra (a, f, h, b);
	tetras_generated += TryGenerateTetra (a, b, h, d);
	tetras_generated += TryGenerateTetra (b, f, g, h);
	tetras_generated += TryGenerateTetra (b, g, h, d);
	tetras_generated += TryGenerateTetra (d, g, b, c);
}

/**
Generate a cube of tetrahedrons.
*/
FiniteElementModel* CubeGenerator::Generate ()
{
	Init ();
	fem_ = new FiniteElementModel;
	for (int i=0; i <3; i++)
		assert (counts_[i] >=2);


	int nod_count = counts_[0] * counts_[1] * counts_[2];
	nods_.SetSize (nod_count);
	for (int i=0; i < nods_.Size (); i++)
		nods_[i] =0;

	cout << "generating cube of " << nod_count << " nodes ... " << flush;

	Vector3 delta;
	for (int i=0; i < 3; i++)
		delta(i) = (counts_[i]-1) ? (dims_[i] / Real(counts_[i] -1.0)) : 0.0;

	/*
	generate the points
	*/
	IntVector3 idx;


	int c =0;
	Real rand_fact = GetNumberSetting ("node-disturbance");
	for (idx[0]=0; idx[0] < counts_[0]; idx[0]++)
		for (idx[1]=0; idx[1] < counts_[1]; idx[1]++)
			for (idx[2]=0; idx[2] < counts_[2]; idx[2]++)
			{
				Vector3 v;
				for (int l = 0; l < 3; l++)
				{
					v(l) = idx[l] * delta (l) - dims_[l] * .5;
					v(l) += rand_fact * random () / RAND_MAX * delta(l);
				}

				Node *n = fem_->MakeNode (v);
				int k = Index3To1 (idx);
				assert (!nods_[k]);
				nods_[k] = n;

				GenerateCubeletVolume (idx);
				if (!(++c %100))
				{
					cout << "[" << c << "]" <<flush;
				}
			}


			cout << "done" << endl;

			fem_->Validate ();
			fem_->UpdateCollisionDetector ();

			return fem_;
}


